#include "bsp_iir.h"
#include "dsp.h"
#include "fpu_filter.h"
#include "math.h"
#include "complex.h"
#include "iir_coeffs1.h"
#include "iir_coeffs2.h"
#include "iir_coeffs3.h"
#include "iir_coeffs4.h"
#include "iir_coeffs5.h"
#include "iir_coeffs6.h"
#include "iir_coeffs7.h"
#include "iir_coeffs8.h"

#define pi 3.1415926





volatile float IIR_output[IIR_SIZE] = {0};
volatile float IIR_input[IIR_SIZE] = {0};

volatile float xBuffer[3] = {0, 0, 0};
volatile float yBuffer[3] = {0, 0, 0};

#ifdef __IIR_COEFFS1_H_
#define FILTER_NUM 1
#endif
#ifdef __IIR_COEFFS2_H_
#define FILTER_NUM 2
#endif
#ifdef __IIR_COEFFS3_H_
#define FILTER_NUM 3
#endif
#ifdef __IIR_COEFFS4_H_
#define FILTER_NUM 4
#endif
#ifdef __IIR_COEFFS5_H_
#define FILTER_NUM 5
#endif
#ifdef __IIR_COEFFS6_H_
#define FILTER_NUM 6
#endif
#ifdef __IIR_COEFFS7_H_
#define FILTER_NUM 7
#endif
#ifdef __IIR_COEFFS8_H_
#define FILTER_NUM 8
#endif


// IIR_f32 object
IIR_f32 iir[FILTER_NUM];
IIR_f32_Handle hnd_iir = &iir[0];
IIR_f32_Handle hnd_buf = &iir[0];

#define CONFIGURE_IIR_FILTER(suffix) do{\
  IIR_f32_setCoefficientsBPtr(&iir[suffix-1], iir##suffix##_coeffs_B); \
  IIR_f32_setCoefficientsAPtr(&iir[suffix-1], iir##suffix##_coeffs_A); \
  IIR_f32_setDelayLinePtr(&iir[suffix-1], iir##suffix##_delayLine); \
  IIR_f32_setInputPtr(&iir[suffix-1], (float*)&iir##suffix##_in); \
  IIR_f32_setOutputPtr(&iir[suffix-1], (float*)&iir##suffix##_out); \
  IIR_f32_setScalePtr(&iir[suffix-1], iir##suffix##_scaleFactors); \
  IIR_f32_setOrder(&iir[suffix-1], (SOS##suffix##_NUM << 1)); \
  IIR_f32_setInitFunction(&iir[suffix-1], (v_pfn_v)IIR_f32_init); \
  IIR_f32_setCalcFunction(&iir[suffix-1], (v_pfn_v)IIR_f32_calc); \
  iir[suffix-1].init(&iir[suffix-1]);}while(0)


void IIR_SwitchFilter(unsigned int filter_index)
{
  filter_index %= FILTER_NUM;
  hnd_iir = hnd_buf+filter_index;
}

void IIR_init()
{
  // Configure the object
#ifdef __IIR_COEFFS1_H_
  CONFIGURE_IIR_FILTER(1);
#endif
#ifdef __IIR_COEFFS2_H_
  CONFIGURE_IIR_FILTER(2);
#endif
#ifdef __IIR_COEFFS3_H_
  CONFIGURE_IIR_FILTER(3);
#endif
#ifdef __IIR_COEFFS4_H_
  CONFIGURE_IIR_FILTER(4);
#endif
#ifdef __IIR_COEFFS5_H_
  CONFIGURE_IIR_FILTER(5);
#endif
#ifdef __IIR_COEFFS6_H_
  CONFIGURE_IIR_FILTER(6);
#endif
#ifdef __IIR_COEFFS7_H_
  CONFIGURE_IIR_FILTER(7);
#endif
#ifdef __IIR_COEFFS8_H_
  CONFIGURE_IIR_FILTER(8);
#endif

}

void IIR_filter_run(void)
{
  // Locals
  uint16_t i;

  iir1_out = FLT_MAX;
  iir1_in = IIR_input[IIR_SIZE - 1];
  hnd_iir->calc(hnd_iir);
  IIR_output[IIR_SIZE - 1] = iir1_out;

}

void IIR_InBuff_add(float value)
{
  int i;
  for (i = 0; i < IIR_SIZE - 1; ++i)
  {
    IIR_input[i] = IIR_input[i + 1];
    IIR_output[i] = IIR_output[i + 1];
  }
  IIR_input[IIR_SIZE - 1] = value;
}

float IIR_OutBuff_GetEnd()
{
  return IIR_output[IIR_SIZE - 1];
}


float IIR_filter(float x)
{
  float y;
  *(hnd_iir->p_output) = FLT_MAX;
  *(hnd_iir->p_input) = x;
  hnd_iir->calc(hnd_iir);
  y = *(hnd_iir->p_output);
  return y;
}


